
#include "bsp.h"
#include "main.h"

#include "SEGGER_RTT.h"
#include "SEGGER_RTT_Conf.h"

#define CLOCKWISE 0

/// @brief 顺时针六步换相表
static const CommutationStep commutation_table[] = 
{
	{0, 0, 1, 0, 1, 1}, // 步骤 0: C+ B- (原步骤5)
	{0, 0, 1, 1, 0, 1}, // 步骤 1: C+ A- (原步骤4)
	{0, 1, 0, 1, 1, 0}, // 步骤 2: B+ A- (原步骤3)
	{0, 1, 0, 0, 1, 1}, // 步骤 3: B+ C- (原步骤2)
	{1, 0, 0, 1, 0, 1}, // 步骤 4: A+ C- (原步骤1)
	{1, 0, 0, 1, 1, 0}, // 步骤 5: A+ B- (原步骤0)
};

/// @brief 逆时针六步换相表
static const CommutationStep commutation_table_cw[] = 
{
    {1, 0, 0, 1, 1, 0}, // 步骤 0: A+ B-，修改pwm_c=2为0
    {1, 0, 0, 1, 0, 1}, // 步骤 1: A+ C-，修改pwm_b=2为0
    {0, 1, 0, 0, 1, 1}, // 步骤 2: B+ C-，修改pwm_a=2为0
    {0, 1, 0, 1, 1, 0}, // 步骤 3: B+ A-，修改pwm_c=2为0
    {0, 0, 1, 1, 0, 1}, // 步骤 4: C+ A-，修改pwm_a=2和pwm_b=2为0
    {0, 0, 1, 0, 1, 1}, // 步骤 5: C+ B-，修改pwm_a=2和pwm_b=2为0
};

extern MotorState motor_state; // 引用外部定义的电机状态
void HoldMotorPosition(uint16_t holding_duty_percent);

void ReadHallAndDetermineStep(void) {
    if (motor_state == 2) 
    {// 停止电机
        Set_PWM_State(0, &htim1, TIM_CHANNEL_1, 0, 1);
        Set_PWM_State(0, &htim15, TIM_CHANNEL_1, 0, 0);
        Set_PWM_State(0, &htim15, TIM_CHANNEL_2, 0, 0);
        HAL_GPIO_WritePin(ENA_GPIO_Port, ENA_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(ENB_GPIO_Port, ENB_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(ENC_GPIO_Port, ENC_Pin, GPIO_PIN_RESET);
        return;
    }
    else if(motor_state == 3)
    {//堵转
        HoldMotorPosition(50);
        SEGGER_RTT_SetTerminal(1);
        SEGGER_RTT_printf(0, "MOTOR_STALLED \r\n");	
    }
    else{

        // 读取霍尔传感器状态（带去抖）
        uint8_t hall_a[3], hall_b[3], hall_c[3];
        for (int i = 0; i < 3; i++) {
            hall_a[i] = HAL_GPIO_ReadPin(HALL_GPIO_PORT, HALL_A_PIN);
            hall_b[i] = HAL_GPIO_ReadPin(HALL_GPIO_PORT, HALL_B_PIN);
            hall_c[i] = HAL_GPIO_ReadPin(HALL_GPIO_PORT, HALL_C_PIN);
            HAL_Delay(10);
        }
        // 确定多数值
        uint8_t a = (hall_a[0] + hall_a[1] + hall_a[2]) >= 2 ? 1 : 0;
        uint8_t b = (hall_b[0] + hall_b[1] + hall_b[2]) >= 2 ? 1 : 0;
        uint8_t c = (hall_c[0] + hall_c[1] + hall_c[2]) >= 2 ? 1 : 0;
        uint8_t hall_state = (a << 2) | (b << 1) | c;

        uint8_t step = 255; // 映射霍尔状态到步骤

        if (motor_state == MOTOR_CLOCKWISE) {
            switch (hall_state) {
                case 0b101: // 5
                    step = 0; // A+ B-
                    break;
                case 0b001: // 1
                    step = 1; // A+ C-
                    break;
                case 0b011: // 3
                    step = 2; // B+ C-
                    break;
                case 0b010: // 2
                    step = 3; // B+ A-
                    break;
                case 0b110: // 6
                    step = 4; // C+ A-
                    break;
                case 0b100: // 4
                    step = 5; // C+ B-
                    break;
                default:
                    break;
            }
        } else if (motor_state == MOTOR_COUNTERCLOCKWISE) {
            switch (hall_state) {
                case 0b001: // 1
                    step = 0;
                    break;
                case 0b101: // 5
                    step = 1;
                    break;
                case 0b100: // 4
                    step = 2;
                    break;
                case 0b110: // 6
                    step = 3;
                    break;
                case 0b010: // 2
                    step = 4;
                    break;
                case 0b011: // 3
                    step = 5;
                    break;
                default:
                    break;
            }
        }

        if (step != 255) {
            uint16_t duty = (motor_state == MOTOR_CLOCKWISE) ? 40 : 40; // 正转35%，反转50%
            // uint16_t duty = (motor_state == MOTOR_CLOCKWISE) ? 80 : 80; // 正转35%，反转50%
            SEGGER_RTT_CustomFunction(1, 0, "Current Step: %d, Hall State: %d, Time: %lu\n", step, hall_state, HAL_GetTick());
            SetCommutationStep(step, duty);
        } else {
            SEGGER_RTT_CustomFunction(1, 0, "Invalid Hall State: %d, Time: %lu\n", hall_state, HAL_GetTick());
        }

    }


}

// 设置 PWM 状态
void Set_PWM_State(uint8_t pwm_state, TIM_HandleTypeDef *htim, uint32_t channel, uint16_t duty_percent, uint8_t is_complementary) 
{
    // 获取 ARR 值
    uint16_t arr = __HAL_TIM_GET_AUTORELOAD(htim);
    // 计算 duty（占空比百分比转换为 CCR 值）
    uint16_t duty = (uint16_t)((float)duty_percent / 100.0f * arr);

    if (pwm_state == 1) { // PWM
        if (is_complementary) {
            HAL_TIMEx_PWMN_Start(htim, channel); // 互补输出
        } else {
            HAL_TIM_PWM_Start(htim, channel); // 标准 PWM
        }
        __HAL_TIM_SET_COMPARE(htim, channel, duty);
    } else { // Low 或 Hi-Z
        if (is_complementary) {
            HAL_TIMEx_PWMN_Stop(htim, channel);
        } else {
            HAL_TIM_PWM_Stop(htim, channel);
        }
        __HAL_TIM_SET_COMPARE(htim, channel, 0);
    }
}
  
// 执行单步换相
void SetCommutationStep(uint8_t step, uint16_t duty_percent) 
{
    if (step >= 6) {
        // 停止电机
        Set_PWM_State(0, &htim1, TIM_CHANNEL_1, 0, 1);
        Set_PWM_State(0, &htim15, TIM_CHANNEL_1, 0, 0);
        Set_PWM_State(0, &htim15, TIM_CHANNEL_2, 0, 0);
        HAL_GPIO_WritePin(ENA_GPIO_Port, ENA_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(ENB_GPIO_Port, ENB_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(ENC_GPIO_Port, ENC_Pin, GPIO_PIN_RESET);
        return;
    }

    // 强制关闭所有 PWM，确保干净状态
    Set_PWM_State(0, &htim1, TIM_CHANNEL_1, 0, 1);  // PWMA
    Set_PWM_State(0, &htim15, TIM_CHANNEL_1, 0, 0); // PWMB
    Set_PWM_State(0, &htim15, TIM_CHANNEL_2, 0, 0); // PWMC

    const CommutationStep *s;
    if (motor_state == MOTOR_CLOCKWISE) {
        s = &commutation_table[step];    // 顺时针
    } else {
        s = &commutation_table_cw[step]; // 逆时针
    }

    // 设置使能信号
    HAL_GPIO_WritePin(ENA_GPIO_Port, ENA_Pin, s->ena ? GPIO_PIN_SET : GPIO_PIN_RESET);
    HAL_GPIO_WritePin(ENB_GPIO_Port, ENB_Pin, s->enb ? GPIO_PIN_SET : GPIO_PIN_RESET);
    HAL_GPIO_WritePin(ENC_GPIO_Port, ENC_Pin, s->enc ? GPIO_PIN_SET : GPIO_PIN_RESET);

    // 设置 PWM 信号
    Set_PWM_State(s->pwm_a, &htim1, TIM_CHANNEL_1, duty_percent, 1);  // PWMA (TIM1_CH1N, PB13)
    Set_PWM_State(s->pwm_b, &htim15, TIM_CHANNEL_1, duty_percent, 0); // PWMB (TIM15_CH1, PB14)
    Set_PWM_State(s->pwm_c, &htim15, TIM_CHANNEL_2, duty_percent, 0); // PWMC (TIM15_CH2, PB15)

    //SEGGER_RTT_CustomFunction(1, 0, "Step %d: PWMA=%d, PWMB=%d, PWMC=%d, ENA=%d, ENB=%d, ENC=%d\n", step, s->pwm_a, s->pwm_b, s->pwm_c, s->ena, s->enb, s->enc);	
}

// 读取霍尔传感器状态并确定六步换向步骤
void ReadHallAndDetermineStep_Demo(void) 
{
    // 读取霍尔传感器状态
    uint8_t hall_a = HAL_GPIO_ReadPin(HALL_GPIO_PORT, HALL_A_PIN);
    uint8_t hall_b = HAL_GPIO_ReadPin(HALL_GPIO_PORT, HALL_B_PIN);
    uint8_t hall_c = HAL_GPIO_ReadPin(HALL_GPIO_PORT, HALL_C_PIN);
    
    // 组合为 3 位值：hall_a << 2 | hall_b << 1 | hall_c
    uint8_t hall_state = (hall_a << 2) | (hall_b << 1) | hall_c;
    
    // 映射霍尔状态到步骤
    uint8_t step = 255; // 无效步骤
    switch (hall_state) {
        case 0b101: // 5
            step = 0; // A+ B-
            break;
        case 0b100: // 4
            step = 1; // A+ C-
            break;
        case 0b110: // 6
            step = 2; // B+ C-
            break;
        case 0b010: // 2
            step = 3; // B+ A-
            break;
        case 0b011: // 3
            step = 4; // C+ A-
            break;
        case 0b001: // 1
            step = 5; // C+ B-
            break;
        default:
            // 无效状态
            break;
    }
    
    // 使用 SEGGER_RTT 输出当前步骤
    if (step != 255) {
        SEGGER_RTT_CustomFunction(1, 0, "Current Step: %d, Hall State: %d\n", step, hall_state);
    } else {
        SEGGER_RTT_CustomFunction(1, 0, "Invalid Hall State: %d\n", hall_state);
    }
}

// In bldc_commutation.c


/**
 * @brief  将电机驱动到预设位置并施加保持力矩
 * @param  holding_duty_percent: 保持力矩对应的PWM占空比 (0-100)
 * @retval None
 */
void HoldMotorPosition(uint16_t holding_duty_percent)
{
    // 选择一个固定的换相步骤作为堵转/保持位置。
    // 这里我们选择逆时针换相表的第0步 (A+ B-)。
    // 你可以选择任何一个有效的步骤。
    const uint8_t HOLDING_STEP = 0;
    
    // 从换相表中获取该步骤的配置
    const CommutationStep *s = &commutation_table_cw[HOLDING_STEP];

    // 强制关闭所有 PWM，确保干净状态
    Set_PWM_State(0, &htim1, TIM_CHANNEL_1, 0, 1);  // PWMA
    Set_PWM_State(0, &htim15, TIM_CHANNEL_1, 0, 0); // PWMB
    Set_PWM_State(0, &htim15, TIM_CHANNEL_2, 0, 0); // PWMC

    // 设置使能信号
    HAL_GPIO_WritePin(ENA_GPIO_Port, ENA_Pin, s->ena ? GPIO_PIN_SET : GPIO_PIN_RESET);
    HAL_GPIO_WritePin(ENB_GPIO_Port, ENB_Pin, s->enb ? GPIO_PIN_SET : GPIO_PIN_RESET);
    HAL_GPIO_WritePin(ENC_GPIO_Port, ENC_Pin, s->enc ? GPIO_PIN_SET : GPIO_PIN_RESET);

    // 设置 PWM 信号
    Set_PWM_State(s->pwm_a, &htim1, TIM_CHANNEL_1, holding_duty_percent, 1);
    Set_PWM_State(s->pwm_b, &htim15, TIM_CHANNEL_1, holding_duty_percent, 0);
    Set_PWM_State(s->pwm_c, &htim15, TIM_CHANNEL_2, holding_duty_percent, 0);

    // (可选) 打印调试信息，但不要太频繁
    // SEGGER_RTT_printf(0, "Holding motor at step %d with duty %d%%\n", HOLDING_STEP, holding_duty_percent);
}


